#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Ultrasonic,     sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     RightMotor,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define WHITE 47
#define BLACK 43

#define CHECK_WHITE_LINE if(SensorValue[Light]>=WHITE)\
								          {\
								          	findWhiteLine = true;\
								          	break;\
								          }

void turnL (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}
void ProportionalLineFollowing ()
{
	while(SensorValue[Light] >= 0 && SensorValue[Light] <= 20)
	{
		motor[LeftMotor] = 0;
		motor[RightMotor] = 50;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 20 && SensorValue[Light] <= 40)
	{
		motor[LeftMotor] = 12;
		motor[RightMotor] = 38;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 40 && SensorValue[Light] <= 60)
	{
		motor[LeftMotor] = 25;
		motor[RightMotor] = 25;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 60 && SensorValue[Light] <= 80)
	{
		motor[LeftMotor] = 38;
		motor[RightMotor] = 12;
		wait1Msec(50);
	}
	while(SensorValue[Light] > 80 && SensorValue[Light] <= 100)
	{
		motor[LeftMotor] = 50;
		motor[RightMotor] = 0;
		wait1Msec(50);
	}
}

task main()
{
	//BEGIN IRFOLLOW
	bool findBeacon = false;
	bool findWhiteLine = false;
		while(!findBeacon)
		{
		   switch (SensorValue[IR1200])
		   {
		     case 1:
		     Backward(25,50);
		     break;

		     case 2:
		     turnL(50,350);
		     findBeacon = true;
		     break;

		     case 0:
		     case 3:
		     case 4:
		     case 5:
		     case 6:
		     case 7:
		     Forward(50,50);
		     break;

		     default:
		   	 break;
		   }
	  }

	  //END IRFOLLOW

	while(SensorValue[Ultrasonic] >= 33 && !findWhiteLine)
	{
		while(SensorValue[IR1200]<5)
		{
		  motor[RightMotor] = 10;
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200]>5)
		{
			motor[LeftMotor] = 10;
		 	CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200]==5)
		{
			if(SensorValue[Light]>=WHITE)
			{
				findWhiteLine = true;
				break;
			}
			else
	    {
	       motor[LeftMotor] = 10;
	       wait1Msec(100);
	       CHECK_WHITE_LINE;
	       motor[RightMotor] = 10;
	       wait1Msec(200);
	       CHECK_WHITE_LINE;
	    }
		}
	//	ProportionalLineFollowing();
	}
	while(SensorValue[Ultrasonic] >= 33)
	{
		Forward(30,50);
	}
}
